#include <iostream>
#include<fstream>


#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh_omp.h>     // fpfh加速计算的omp(多核并行计算)
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/common_headers.h>
#include <pcl/registration/ia_ransac.h>//采样一致性
#include <pcl/registration/icp.h>      //icp配准
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>

using namespace std;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;


void extract_keypoint(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& keypoint,
                      float LeafSize = 0.4, float radius = 0.8, float threshold = 5) // 参数分别为：体素格网的大小，法向量计算半径，夹角阈值（度）0.4,0.8,5
{
    //体素下采样
    pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_down(new pcl::PointCloud<pcl::PointXYZRGB>);
    voxel_grid.setInputCloud(cloud);
    voxel_grid.setLeafSize(LeafSize, LeafSize, LeafSize);
    voxel_grid.filter(*pcd_down);

    //计算每一个点的法向量
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> n;
    n.setInputCloud(pcd_down);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    n.setSearchMethod(tree);
    //设置KD树搜索半径
    //n.setRadiusSearch (0.1);
    n.setKSearch(10);
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    n.compute(*normals);

    float Angle = 0.0;
    float Average_Sum_AngleK = 0.0;//定义邻域内K个点法向量夹角的平均值
    vector<int>indexes;
    //--------------计算法向量夹角及夹角均值----------------
    pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;  //建立kdtree对象
    kdtree.setInputCloud(pcd_down); //设置需要建立kdtree的点云指针

    vector<int> pointIdxRadiusSearch;  //保存每个近邻点的索引
    vector<float> pointRadiusSquaredDistance;  //保存每个近邻点与查找点之间的欧式距离平方
    pcl::PointXYZRGB searchPoint;
    for (size_t i = 0; i < pcd_down->points.size(); ++i) {
        searchPoint = pcd_down->points[i];
        if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
        {
            float Sum_AngleK = 0.0;//定义K个邻近的点法向夹角之和
            /*计算法向量的夹角*/
            for (size_t m = 0; m < pointIdxRadiusSearch.size(); ++m) {

                Eigen::Vector3f
                        v1(normals->points[i].data_n[0],
                        normals->points[i].data_n[1],
                        normals->points[i].data_n[2]),

                        v2(normals->points[pointIdxRadiusSearch[m]].data_n[0],
                        normals->points[pointIdxRadiusSearch[m]].data_n[1],
                        normals->points[pointIdxRadiusSearch[m]].data_n[2]);

                Angle = pcl::getAngle3D(v1, v2, true);

            }
            Sum_AngleK += Angle;//邻域夹角之和
            Average_Sum_AngleK = Sum_AngleK / pointIdxRadiusSearch.size();//邻域夹角均值

            //-----------------提取特征点--------------------
            float t = pcl::deg2rad(threshold);
            if (Average_Sum_AngleK > t) {
                indexes.push_back(i);
            }
        }
    }
    pcl::copyPointCloud(*pcd_down, indexes, *keypoint);

    cout << "提取的特征点个数:" << keypoint->points.size() << endl;

};

pcl::PointCloud<pcl::FPFHSignature33>::Ptr compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& keypoint)
{
    // 法向量估计
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> n;
    n.setInputCloud(keypoint);
    n.setSearchMethod(tree);
    n.setKSearch(10);
    //n.setRadiusSearch(0.0001);
    n.compute(*normals);

    //------------------FPFH估计-------------------------
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);
    pcl::FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> f;
    f.setNumberOfThreads(8); //指定8核计算
    f.setInputCloud(keypoint);
    f.setInputNormals(normals);
    f.setSearchMethod(tree);
    f.setKSearch(10);
    //f.setRadiusSearch(0.0012);
    f.compute(*fpfh);
    return fpfh;
}

void pretreat(PointCloud::Ptr& pcd_cloud, PointCloud::Ptr& pcd_down, float LeafSize = 0.05) {
    //去除NAN点
    std::vector<int> indices_src; //保存去除的点的索引
    pcl::removeNaNFromPointCloud(*pcd_cloud, *pcd_cloud, indices_src);
    std::cout << "remove *cloud_source nan" << endl;
    //下采样滤波
    pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid;
    voxel_grid.setLeafSize(LeafSize, LeafSize, LeafSize);
    voxel_grid.setInputCloud(pcd_cloud);
    voxel_grid.filter(*pcd_down);
    cout << "down size *cloud from " << pcd_cloud->size() << "to" << pcd_down->size() << endl;
};


void help(char *appname) {
    printf("Usage: %s <source.pcd> <target.pcd> <outfn.pcd>\n", appname);
    printf("pcl_icp m6_filter.pcd m7_filter.pcd m6target.pcd\n");
}

int main(int argc, char *argv[])
{
    if (argc < 4) {
        help(argv[0]);
        return -1;
    }

    string srcfn = argv[1];
    string targetfn = argv[2];
    string outfn = argv[3];

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr source(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::io::loadPCDFile(srcfn, *source);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr target(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::io::loadPCDFile(targetfn, *target);
    if (source->empty() || target->empty()) // 使用empty()函数判断点云是否加载成功
    {
        cout << "请确认点云文件名称是否正确" << endl;
        return -1;
    }
    else
    {
        cout << "从目标点云读取 " << target->size() << " 个点" << endl;
        cout << "从源点云中读取 " << source->size() << " 个点" << endl;
    }


    //1、 提取特征点
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr s_k(new pcl::PointCloud<pcl::PointXYZRGB>);
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr t_k(new pcl::PointCloud<pcl::PointXYZRGB>);
     //extract_keypoint(source, s_k, 0.005, 0.01, 5);
     //extract_keypoint(target, t_k, 0.005, 0.01, 5);
     extract_keypoint(source, s_k, 0.004, 0.01, 4); // 按照leaf_size 合成一个点， radius 大约是 leaf_size 两倍
     extract_keypoint(target, t_k, 0.004, 0.01, 4);

     //return 0;
     //2、计算源点云和目标点云的FPFH
     pcl::PointCloud<pcl::FPFHSignature33>::Ptr sk_fpfh = compute_fpfh_feature(s_k);
     pcl::PointCloud<pcl::FPFHSignature33>::Ptr tk_fpfh = compute_fpfh_feature(t_k);
     cout << "calcute FPFH finish!" << endl;

    //3、SAC配准
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZRGB, pcl::PointXYZRGB, pcl::FPFHSignature33> scia;
    scia.setInputSource(s_k);
    scia.setInputTarget(t_k);
    scia.setSourceFeatures(sk_fpfh);
    scia.setTargetFeatures(tk_fpfh);
    //scia.setMinSampleDistance(0.0001);
    scia.setNumberOfSamples(50); //设置每次迭代计算中使用的样本数量（可省）, 可节省时间
    scia.setCorrespondenceRandomness(30);  //设置计算协方差时选择多少近邻点，该值越大，协防差越精确， 0.1
    scia.setEuclideanFitnessEpsilon(0.001);
    scia.setTransformationEpsilon(1e-10);
    scia.setRANSACIterations(30);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr sac_result(new pcl::PointCloud<pcl::PointXYZRGB>);
    scia.align(*sac_result);
    //pcl::io::savePCDFileASCII("C:/Users/lrk/Desktop/test_data/new4_13/point_pcd/m6sac.pcd", *sac_result);
    std::cout << "sac has converged:" << scia.hasConverged() << "  score: " << scia.getFitnessScore() << endl;
    Eigen::Matrix4f sac_trans;
    sac_trans = scia.getFinalTransformation();
    std::cout << sac_trans << endl;

    clock_t start = clock();
    float value = 0.0005; // 0.0005;  // 下采样参数
    PointCloud::Ptr cloud_src(new PointCloud);
    PointCloud::Ptr cloud_tar(new PointCloud);
    pretreat(source, cloud_src, value);
    pretreat(target, cloud_tar, value);

    //4、KD树改进的ICP配准
    pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
    //kdTree 加速搜索
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZRGB>);
    tree1->setInputCloud(cloud_src);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZRGB>);
    tree2->setInputCloud(cloud_tar);
    icp.setSearchMethodSource(tree1);
    icp.setSearchMethodTarget(tree2);
    icp.setInputSource(cloud_src);
    icp.setInputTarget(cloud_tar);
    icp.setMaxCorrespondenceDistance(0.0005); //设置最大对应点的欧式距离，只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对
    icp.setMaximumIterations(200);                  //设置最大的迭代次数。迭代停止条件之一 ，几十上百都可能出现。
    icp.setTransformationEpsilon(1e-10);    //设置前后两次迭代的转换矩阵的最大容差（epsilion），一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止。0
    icp.setEuclideanFitnessEpsilon(0.00001);   //前后两次迭代误差的差值。收敛条件是均方误差和小于阈值， 停止迭代。
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr icp_result(new pcl::PointCloud<pcl::PointXYZRGB>);
    icp.align(*icp_result, sac_trans);
    pcl::transformPointCloud(*source, *icp_result, icp.getFinalTransformation());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr new_target(new pcl::PointCloud<pcl::PointXYZRGB>);
    *new_target = *icp_result + *target;
    pcl::io::savePCDFileASCII(outfn, *new_target);
    // 5、可视化
    //visualize_registration(source, target, icp_result);


    return 0;
}
